#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "learning_action/dishAction.h"

typedef actionlib::SimpleActionServer<learning_action::dishAction> Server;

// 收到action的goal后调用的回调函数
// void execute(const learning_action::dishGoalConstPtr &goal, Server *as)
// {
//     ros::Rate r(1);
//     learning_action::dishFeedback feedback;

//     ROS_INFO("Dishwasher %d is working.", goal->dishwasher_id);

//     // 假设洗盘子的进度，并且按照1hz的频率发布进度feedback
//     for (int i = 1; i <= 10; i++)
//     {
//         feedback.percent_complete = i * 10;
//         as->publishFeedback(feedback);
//         r.sleep();
//     }

//     // 当action完成后，向客户端返回结果
//     ROS_INFO("Dishwasher %d finish working.", goal->dishwasher_id);
//     as->setSucceeded();
// }
class Test
{
private:
    ros::NodeHandle _nh;

public:
    Test() {}
    ~Test() {}

    void execute(const learning_action::dishGoalConstPtr &goal, Server *as)
    {
        ros::Rate r(1);
        learning_action::dishFeedback feedback;

        ROS_INFO("Dishwasher %d is working.", goal->dishwasher_id);

        // 假设洗盘子的进度，并且按照1hz的频率发布进度feedback
        for (int i = 1; i <= 10; i++)
        {
            feedback.percent_complete = i * 10;
            as->publishFeedback(feedback);
            r.sleep();
        }

        // 当action完成后，向客户端返回结果
        ROS_INFO("Dishwasher %d finish working.", goal->dishwasher_id);
        as->setSucceeded();
    }
    void run()
    {
        boost::shared_ptr<::learning_action::dishGoal const> dishGoalConstPtr;
        // auto fun = boost::bind(&Test::execute, this, _1);
        // boost::function<void(const dishGoalConstPtr &)> fun = boost::bind(&Test::execute, this);
        // 定义一个服务器
        Server server(_nh, "do_dishes", boost::bind(&Test::execute, this, _1), false);

        // 服务器开始运行
        server.start();
    }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "do_dishes_server");
    ros::NodeHandle n;
    Test t;
    t.run();

    ros::spin();

    return 0;
}